#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Odometry.h>
#include <cmath>

// 全局变量：存储当前小车位置（从odom话题获取）
geometry_msgs::Point current_pos;

// 【回调函数】订阅/odom话题，提取小车位置信息
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    // 从Odometry消息中获取位置（x/y/z），赋值给全局变量current_pos
    current_pos.x = msg->pose.pose.position.x;
    current_pos.y = msg->pose.pose.position.y;
    current_pos.z = msg->pose.pose.position.z;  // 若小车在平面运动，z通常为0
}

int main(int argc, char** argv) {
    // 1. 初始化ROS节点，节点名为"points_and_lines"
    ros::init(argc, argv, "display_in_rviz");
    ros::NodeHandle nh;  // 创建节点句柄，用于ROS通信

    // 2. 创建Marker发布者：话题为"visualization_marker"，队列大小10（防止消息堆积）
    ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>(
        "visualization_marker", 10
    );

    // 3. 创建Odometry订阅者：订阅"/odom"话题，队列大小10，回调函数为odomCallback
    ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
        "/odom", 10, odomCallback
    );

    // 4. 设置循环频率：30Hz（每秒更新30次Marker，平衡流畅度与性能）
    ros::Rate loop_rate(30);

    // 5. 定义3种Marker对象：
    // - points：离散点（标记小车经过的每个位置）
    // - line_strip：连续线带（连接所有点，形成轨迹）
    // - line_list：独立线列表（可画多条独立线，此处用于辅助标记）
    visualization_msgs::Marker points, line_strip, line_list;

    // 6. 记录上一帧位置：用于判断小车是否移动，避免重复添加相同点
    geometry_msgs::Point last_pos = current_pos;

    while (ros::ok()) {  // ROS节点正常运行时循环
        // 仅当小车位置变化时，才更新Marker（避免无效计算）
        if (current_pos.x != last_pos.x || current_pos.y != last_pos.y) {
            // -------------------------- 【Marker公共配置】--------------------------
            // 坐标系：建议与odom话题一致（原代码/my_frame可能需改为/odom，否则RViz看不到）
            points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/odom";
            // 时间戳：使用当前时间，确保与RViz时间同步
            points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
            // 命名空间（ns）：区分同一话题下的不同Marker，避免冲突
            points.ns = line_strip.ns = line_list.ns = "display_in_rviz";
            // 动作：ADD（添加/更新Marker），若为DELETE则删除Marker
            points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
            // 姿态：四元数（w=1表示无旋转，小车轨迹默认不需要旋转）
            points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

            // -------------------------- 【MarkerID与类型配置】--------------------------
            // ID：同一ns下，不同ID对应不同Marker（避免覆盖）
            points.id = 0;
            line_strip.id = 1;
            line_list.id = 2;
            // 类型：3种不同的可视化类型
            points.type = visualization_msgs::Marker::POINTS;       // 离散点
            line_strip.type = visualization_msgs::Marker::LINE_STRIP; // 连续线带（点按顺序连接）
            line_list.type = visualization_msgs::Marker::LINE_LIST;   // 独立线列表（每2个点组成一条线）

            // -------------------------- 【Marker缩放配置】--------------------------
            // 缩放（scale）：不同类型Marker含义不同
            // - POINTS：x=点的宽度，y=点的高度（单位：米）
            points.scale.x = 0.2;  // 点的宽度：0.2m
            points.scale.y = 0.2;  // 点的高度：0.2m
            // - LINE_STRIP/LINE_LIST：仅x生效，代表线的宽度（单位：米）
            line_strip.scale.x = 0.1;  // 轨迹线宽度：0.1m
            line_list.scale.x = 0.1;    // 辅助线宽度：0.1m

            // -------------------------- 【Marker颜色配置（核心修改）】--------------------------
            // 颜色格式：RGB（0~1） + alpha（透明度，0=完全透明，1=完全不透明）
            // 1. 离散点（points）：修改为橙色（原绿色）
            points.color.r = 1.0f;    // 红色分量：1.0
            points.color.g = 0.5f;    // 绿色分量：0.5
            points.color.b = 0.0f;    // 蓝色分量：0.0
            points.color.a = 1.0f;    // 透明度：1.0（不透明）

            // 2. 连续轨迹（line_strip）：修改为深绿色（原蓝色）
            line_strip.color.r = 0.0f;
            line_strip.color.g = 0.8f;
            line_strip.color.b = 0.0f;
            line_strip.color.a = 1.0f;

            // 3. 独立线列表（line_list）：修改为紫色（原红色）
            line_list.color.r = 0.6f;
            line_list.color.g = 0.2f;
            line_list.color.b = 1.0f;
            line_list.color.a = 1.0f;

            // -------------------------- 【添加位置数据到Marker】--------------------------
            // 1. 离散点：添加当前位置到points
            points.points.push_back(current_pos);
            // 2. 连续轨迹：添加当前位置到line_strip（自动连接所有点）
            line_strip.points.push_back(current_pos);
            // 3. 独立线列表：需成对添加点（每2个点画一条线）
            // 此处优化：原代码添加2次current_pos会画"点到自身"的无效线，改为"当前点+上一点"
            line_list.points.push_back(last_pos);       // 线的起点：上一位置
            line_list.points.push_back(current_pos);    // 线的终点：当前位置

            // -------------------------- 【发布Marker】--------------------------
            marker_pub.publish(points);       // 发布离散点
            marker_pub.publish(line_strip);   // 发布轨迹线
            marker_pub.publish(line_list);    // 发布辅助线

            // 更新"上一位置"：为下一帧判断做准备
            last_pos = current_pos;
        }

        // 处理ROS回调（必须调用，否则无法接收/odom话题数据）
        ros::spinOnce();
        // 按循环频率休眠（30Hz即约33ms休眠一次）
        loop_rate.sleep();
    }

    return 0;
}
